
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_mission.c
  * @author     baiyang
  * @date       2022-5-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_mission.h"

#include <float.h>
#include <stdio.h>
#include <string.h>
#include <assert.h>

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
#pragma pack(1)

struct Packed_Location_Option_Flags {
    uint8_t relative_alt : 1;           // 1 if altitude is relative to home
    uint8_t unused1      : 1;           // unused flag (defined so that loiter_ccw uses the correct bit)
    uint8_t loiter_ccw   : 1;           // 0 if clockwise, 1 if counter clockwise
    uint8_t terrain_alt  : 1;           // this altitude is above terrain
    uint8_t origin_alt   : 1;           // this altitude is above ekf origin
    uint8_t loiter_xtrack : 1;          // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
    uint8_t type_specific_bit_0 : 1;    // each mission item type can use this for storing 1 bit of extra data
};

struct PackedLocation {
    union {
        struct Packed_Location_Option_Flags flags;                    ///< options bitmask (1<<0 = relative altitude)
        uint8_t options;                                /// allows writing all flags to eeprom as one byte
    };
    // by making alt 24 bit we can make p1 in a command 16 bit,
    // allowing an accurate angle in centi-degrees. This keeps the
    // storage cost per mission item at 15 bytes, and allows mission
    // altitudes of up to +/- 83km
    int32_t alt:24;                                     ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
    int32_t lat;                                        ///< param 3 - Latitude * 10**7
    int32_t lng;                                        ///< param 4 - Longitude * 10**7
};
#pragma pack()

union PackedContent {
    // location
    struct PackedLocation location;      // Waypoint location

    // raw bytes, for reading/writing to eeprom. Note that only 10
    // bytes are available if a 16 bit command ID is used
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    uint8_t bytes[12];
#else
    uint8_t bytes[13];
#endif
};

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
_Static_assert(sizeof(union PackedContent) == 12, "PackedContent wrong size");
#else
static_assert(sizeof(union PackedContent) == 13, "PackedContent wrong size");
#endif
/*---------------------------------prototype----------------------------------*/
static void mission_assign_param();
static void mission_check_eeprom_version();
static bool mission_stored_in_location(uint16_t id);
static void mission_param_set_and_save(const char* param_name, double value);
static void mission_init_jump_tracking();
static int16_t mission_get_jump_times_run(const mission_command_t cmd);
static void mission_increment_jump_times_run(mission_command_t cmd, bool send_gcs_msg);
static void mission_complete();
static bool mission_advance_current_nav_cmd(uint16_t starting_index);
static void mission_advance_current_do_cmd();
static bool mission_get_next_cmd(uint16_t start_index, mission_command_t cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg);
static bool mission_get_next_do_cmd(uint16_t start_index, mission_command_t cmd);
static bool mission_start_command(const mission_command_t cmd);
static bool mission_verify_command(const mission_command_t cmd);
static bool mission_is_landing_type_cmd(uint16_t id);
static bool mission_is_takeoff_type_cmd(uint16_t id);
static bool mission_calc_rewind_pos(mission_command_t rewind_cmd);
static bool mission_is_takeoff_next(uint16_t cmd_index);
static void mission_update_exit_position(void);
static bool mission_distance_to_landing(uint16_t index, float *tot_distance, Location prev_loc);
static bool mission_calculate_contains_terrain_alt_items(void);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    PARAM_DEFINE_INT16(MIS_TOTAL,   0),
    PARAM_DEFINE_INT8(MIS_RESTART,  AP_MISSION_RESTART_DEFAULT),
    PARAM_DEFINE_INT16(MIS_OPTIONS, AP_MISSION_OPTIONS_DEFAULT),
};
PARAM_GROUP_DEFINE(MIS, var_info);

static struct mb_mission mission;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
mb_mission_t mission_get_singleton()
{
    return &mission;
}

void mission_init(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn)
{
    mission_assign_param();

    StorageAccess_init(&mission._storage, StorageMission);

    mission._cmd_start_fn = cmd_start_fn;
    mission._cmd_verify_fn = cmd_verify_fn;
    mission._mission_complete_fn = mission_complete_fn;
    mission._prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
    mission._prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
    mission._prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;

    // clear commands
    mission._nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._do_cmd.index = AP_MISSION_CMD_INDEX_NONE;

    // check_eeprom_version - checks version of missions stored in eeprom matches this library
    // command list will be cleared if they do not match
    mission_check_eeprom_version();

    // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission.
    if (AP_MISSION_MASK_MISSION_CLEAR & mission._options) {
        gcs_send_text(MAV_SEVERITY_INFO, "Clearing Mission");
        mission_clear();
    }

    mission._last_change_time_ms = time_millis();
}

/// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission
///     should be called at 10hz or higher
void mission_update()
{
    // exit immediately if not running or no mission commands
    if (mission._flags.state != MISSION_RUNNING || mission._cmd_total == 0) {
        return;
    }

    mission_update_exit_position();

    // save persistent waypoint_num for watchdog restore
    //hal.util->persistent_data.waypoint_num = mission._nav_cmd.index;

    // check if we have an active nav command
    if (!mission._flags.nav_cmd_loaded || mission._nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
        // advance in mission if no active nav command
        if (!mission_advance_current_nav_cmd(0)) {
            // failure to advance nav command means mission has completed
            mission_complete();
            return;
        }
    } else {
        // run the active nav command
        if (mission_verify_command(&mission._nav_cmd)) {
            // market _nav_cmd as complete (it will be started on the next iteration)
            mission._flags.nav_cmd_loaded = false;
            // immediately advance to the next mission command
            if (!mission_advance_current_nav_cmd(0)) {
                // failure to advance nav command means mission has completed
                mission_complete();
                return;
            }
        }
    }

    // check if we have an active do command
    if (!mission._flags.do_cmd_loaded) {
        mission_advance_current_do_cmd();
    } else {
        // check the active do command
        if (mission_verify_command(&mission._do_cmd)) {
            // mark _do_cmd as complete
            mission._flags.do_cmd_loaded = false;
        }
    }
}

/// clear - clears out mission
///     returns true if mission was running so it could not be cleared
bool mission_clear()
{
    // do not allow clearing the mission while it is running
    if (mission._flags.state == MISSION_RUNNING) {
        return false;
    }

    // remove all commands
    mission_truncate(0);

    // clear index to commands
    mission._nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._flags.nav_cmd_loaded = false;
    mission._flags.do_cmd_loaded = false;

    // return success
    return true;
}

/// trucate - truncate any mission items beyond index
void mission_truncate(uint16_t index)
{
    StorageAccess_erase(&mission._storage);

    if ((unsigned)mission._cmd_total > index) {
        mission_param_set_and_save("MIS_TOTAL", index);
        mission._last_change_time_ms = time_millis();
    }
}

/// start - resets current commands to point to the beginning of the mission
///     To-Do: should we validate the mission first and return true/false?
void mission_start()
{
    mission._flags.state = MISSION_RUNNING;

    mission_reset(); // reset mission to the first command, resets jump tracking

    // advance to the first command
    if (!mission_advance_current_nav_cmd(0)) {
        // on failure set mission complete
        mission_complete();
    }
}

/// stop - stops mission execution.  subsequent calls to update() will have no effect until the mission is started or resumed
void mission_stop()
{
    mission._flags.state = MISSION_STOPPED;
}

/// resume - continues the mission execution from where we last left off
///     previous running commands will be re-initialized
void mission_resume()
{
    // if mission had completed then start it from the first command
    if (mission._flags.state == MISSION_COMPLETE) {
        mission_start();
        return;
    }

    // if mission had stopped then restart it
    if (mission._flags.state == MISSION_STOPPED) {
        mission._flags.state = MISSION_RUNNING;

        // if no valid nav command index restart from beginning
        if (mission._nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
            mission_start();
            return;
        }
    }

    // ensure cache coherence
    if (!mission_read_cmd_from_storage(mission._nav_cmd.index, &mission._nav_cmd)) {
        // if we failed to read the command from storage, then the command may have
        // been from a previously loaded mission it is illogical to ever resume
        // flying to a command that has been excluded from the current mission
        mission_start();
        return;
    }

    // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
    if (mission._repeat_dist > 0 && mission._wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) {
        // if not already in a resume state calculate the position to rewind to
        struct Mission_Command tmp_cmd;
        if (!mission._flags.resuming_mission && mission_calc_rewind_pos(&tmp_cmd)) {
            mission._resume_cmd = tmp_cmd;
        }

        // resume mission to rewound position
        if (mission._resume_cmd.index != AP_MISSION_CMD_INDEX_NONE && mission_start_command(&mission._resume_cmd)) {
            mission._nav_cmd = mission._resume_cmd;
            mission._flags.nav_cmd_loaded = true;
            // set flag to prevent history being re-written
            mission._flags.resuming_mission = true;
            return;
        }
    }

    // restart active navigation command. We run these on resume()
    // regardless of whether the mission was stopped, as we may be
    // re-entering AUTO mode and the nav_cmd callback needs to be run
    // to setup the current target waypoint

    if (mission._flags.do_cmd_loaded && mission._do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
        // restart the active do command, which will also load the nav command for us
        mission_set_current_cmd(mission._do_cmd.index, false);
    } else if (mission._flags.nav_cmd_loaded) {
        // restart the active nav command
        mission_set_current_cmd(mission._nav_cmd.index, false);
    }

    // Note: if there is no active command then the mission must have been stopped just after the previous nav command completed
    //      update will take care of finding and starting the nav command
}

/// check mission starts with a takeoff command
bool mission_starts_with_takeoff_cmd()
{
    uint16_t cmd_index = mission._restart ? AP_MISSION_CMD_INDEX_NONE : mission._nav_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    }
    return mission_is_takeoff_next(cmd_index);
}

/*
    return true if MIS_OPTIONS is set to allow continue of mission
    logic after a land and the next waypoint is a takeoff. If this
    is false then after a landing is complete the vehicle should 
    disarm and mission logic should stop
*/
bool mission_continue_after_land_check_for_takeoff()
{
    if (!mission_continue_after_land()) {
        return false;
    }
    if (mission._nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
        return false;
    }
    return mission_is_takeoff_next(mission._nav_cmd.index+1);
}

bool mission_continue_after_land(void)
{
    return (mission._options & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0;
}

/// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
void mission_start_or_resume()
{
    if (mission._restart == 1 && !mission._force_resume) {
        mission_start();
    } else {
        mission_resume();
        mission._force_resume = false;
    }
}

/// reset - reset mission to the first command
void mission_reset()
{
    mission._flags.nav_cmd_loaded  = false;
    mission._flags.do_cmd_loaded   = false;
    mission._flags.do_cmd_all_done = false;
    mission._flags.in_landing_sequence = false;
    mission._nav_cmd.index         = AP_MISSION_CMD_INDEX_NONE;
    mission._do_cmd.index          = AP_MISSION_CMD_INDEX_NONE;
    mission._prev_nav_cmd_index    = AP_MISSION_CMD_INDEX_NONE;
    mission._prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
    mission._prev_nav_cmd_id       = AP_MISSION_CMD_ID_NONE;
    mission_init_jump_tracking();
    mission_reset_wp_history();
}

// reset the mission history to prevent recalling previous mission histories after a mission restart.
void mission_reset_wp_history(void)
{
    for (uint8_t i = 0; i<AP_MISSION_MAX_WP_HISTORY; i++) {
        mission._wp_index_history[i] = AP_MISSION_CMD_INDEX_NONE;
    }
    mission._resume_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._flags.resuming_mission = false;
    mission._repeat_dist = 0;
}

/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
bool mission_is_nav_cmd(const mission_command_t cmd)
{
    // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions
    return (cmd->id <= MAV_CMD_NAV_LAST ||
            cmd->id == MAV_CMD_NAV_SET_YAW_SPEED ||
            cmd->id == MAV_CMD_NAV_SCRIPT_TIME ||
            cmd->id == MAV_CMD_NAV_ATTITUDE_TIME);
}

/// load_cmd_from_storage - load command from storage
///     true is return if successful
bool mission_read_cmd_from_storage(uint16_t index, mission_command_t cmd)
{
    //WITH_SEMAPHORE(_rsem);

    // special handling for command #0 which is home
    if (index == 0) {
        memset(cmd, 0, sizeof(struct Mission_Command));
        cmd->id = MAV_CMD_NAV_WAYPOINT;
        cmd->content.location = get_ahrs_view()->home_loc;
        return true;
    }

    if (index >= (unsigned)mission._cmd_total) {
        return false;
    }

    // ensure all bytes of cmd are zeroed
    memset(cmd, 0, sizeof(struct Mission_Command));

    // Find out proper location in memory by using the start_byte position + the index
    // we can load a command, we don't process it yet
    // read WP position
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
#else
    const uint16_t pos_in_storage = 4 + (index * (AP_MISSION_EEPROM_COMMAND_SIZE + 1));
#endif

    union PackedContent packed_content = {0};

    const uint8_t b1 = StorageAccess_read_byte(&mission._storage, pos_in_storage);
    if (b1 == 0) {
        cmd->id = StorageAccess_read_uint16(&mission._storage, pos_in_storage+1);
        cmd->p1 = StorageAccess_read_uint16(&mission._storage, pos_in_storage+3);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        StorageAccess_read_block(&mission._storage, packed_content.bytes, pos_in_storage+5, 10);
#else
        StorageAccess_read_block(&mission._storage, packed_content.bytes, pos_in_storage+5, 10 + 1);
#endif
    } else {
        cmd->id = b1;
        cmd->p1 = StorageAccess_read_uint16(&mission._storage, pos_in_storage+1);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        StorageAccess_read_block(&mission._storage, packed_content.bytes, pos_in_storage+3, 12);
#else
        StorageAccess_read_block(&mission._storage, packed_content.bytes, pos_in_storage+3, 12 + 1);
#endif
    }

    if (mission_stored_in_location(cmd->id)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        // NOTE!  no 16-bit command may be stored_in_location as only
        // 10 bytes are available for storage and lat/lon/alt required
        // 4*sizeof(float) == 12 bytes of storage.
        if (b1 == 0) {
            console_panic("May not store location for 16-bit commands");
        }
#endif
        // Location is not PACKED; field-wise copy it:
        cmd->content.location.relative_alt = packed_content.location.flags.relative_alt;
        cmd->content.location.loiter_ccw = packed_content.location.flags.loiter_ccw;
        cmd->content.location.terrain_alt = packed_content.location.flags.terrain_alt;
        cmd->content.location.origin_alt = packed_content.location.flags.origin_alt;
        cmd->content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack;
        cmd->content.location.alt = packed_content.location.alt;
        cmd->content.location.lat = packed_content.location.lat;
        cmd->content.location.lng = packed_content.location.lng;

        if (packed_content.location.flags.type_specific_bit_0) {
            cmd->type_specific_bits = 1U << 0;
        }
    } else {
        // all other options in Content are assumed to be packed:
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        _Static_assert(sizeof(cmd->content) >= 12,
                      "content is big enough to take bytes");

        // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
        memcpy((void *)&cmd->content, packed_content.bytes, 12);
#else
        static_assert(sizeof(cmd->content) >= 12,
                      "content is big enough to take bytes");

        // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
        memcpy((void *)&cmd->content, packed_content.bytes, 12+1);
#endif
    }

    // set command's index to it's position in eeprom
    cmd->index = index;

    // return success
    return true;
}

/// write_cmd_to_storage - write a command to storage
///     index is used to calculate the storage location
///     true is returned if successful
bool mission_write_cmd_to_storage(uint16_t index, const mission_command_t cmd)
{
    //WITH_SEMAPHORE(_rsem);

    // range check cmd's index
    if (index >= mission_num_commands_max()) {
        return false;
    }

    union PackedContent packed = {0};

    if (mission_stored_in_location(cmd->id)) {
        // Location is not PACKED; field-wise copy it:
        packed.location.flags.relative_alt = cmd->content.location.relative_alt;
        packed.location.flags.loiter_ccw = cmd->content.location.loiter_ccw;
        packed.location.flags.terrain_alt = cmd->content.location.terrain_alt;
        packed.location.flags.origin_alt = cmd->content.location.origin_alt;
        packed.location.flags.loiter_xtrack = cmd->content.location.loiter_xtrack;
        packed.location.flags.type_specific_bit_0 = cmd->type_specific_bits & (1U<<0);
        packed.location.alt = cmd->content.location.alt;
        packed.location.lat = cmd->content.location.lat;
        packed.location.lng = cmd->content.location.lng;
    } else {
        // all other options in Content are assumed to be packed:
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        _Static_assert(sizeof(packed.bytes) >= 12,
                      "packed.bytes is big enough to take content");

        memcpy(packed.bytes, &cmd->content, 12);
#else
        static_assert(sizeof(packed.bytes) >= 13,
                      "packed.bytes is big enough to take content");

        memcpy(packed.bytes, &cmd->content, 12 + 1);
#endif
    }

    // calculate where in storage the command should be placed
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
#else
    uint16_t pos_in_storage = 4 + (index * (AP_MISSION_EEPROM_COMMAND_SIZE+1));
#endif

    if (cmd->id < 256) {
        StorageAccess_write_byte(&mission._storage, pos_in_storage, cmd->id);
        StorageAccess_write_uint16(&mission._storage, pos_in_storage+1, cmd->p1);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        StorageAccess_write_block(&mission._storage, pos_in_storage+3, packed.bytes, 12);
#else
        StorageAccess_write_block(&mission._storage, pos_in_storage+3, packed.bytes, 12 + 1);
#endif

    } else {
        // if the command ID is above 256 we store a 0 followed by the 16 bit command ID
        StorageAccess_write_byte(&mission._storage, pos_in_storage, 0);
        StorageAccess_write_uint16(&mission._storage, pos_in_storage+1, cmd->id);
        StorageAccess_write_uint16(&mission._storage, pos_in_storage+3, cmd->p1);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        StorageAccess_write_block(&mission._storage, pos_in_storage+5, packed.bytes, 10);
#else
        StorageAccess_write_block(&mission._storage, pos_in_storage+5, packed.bytes, 10 + 1);
#endif
    }

    // remember when the mission last changed
    mission._last_change_time_ms = time_millis();

    // return success
    return true;
}

/*
  return total number of commands that can fit in storage space
 */
uint16_t mission_num_commands_max(void)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    // -4 to remove space for eeprom version number
    return (StorageAccess_size(&mission._storage) - 4) / AP_MISSION_EEPROM_COMMAND_SIZE;
#else
    // -4 to remove space for eeprom version number
    return (StorageAccess_size(&mission._storage) - 4) / (AP_MISSION_EEPROM_COMMAND_SIZE + 1);
#endif
}

// set_current_cmd - jumps to command specified by index
// bool rewind = false
bool mission_set_current_cmd(uint16_t index, bool rewind)
{
    // read command to check for DO_LAND_START
    struct Mission_Command cmd;
    if (!mission_read_cmd_from_storage(index, &cmd) || (cmd.id != MAV_CMD_DO_LAND_START)) {
        mission._flags.in_landing_sequence = false;
    }

    // mission command has been set and not as rewind command, don't track history.
    if (!rewind) {
        mission_reset_wp_history();
    }

    // sanity check index and that we have a mission
    if (index >= (unsigned)mission._cmd_total || mission._cmd_total == 1) {
        return false;
    }

    // stop the current running do command
    mission._do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._flags.do_cmd_loaded = false;
    mission._flags.do_cmd_all_done = false;

    // stop current nav cmd
    mission._flags.nav_cmd_loaded = false;

    // if index is zero then the user wants to completely restart the mission
    if (index == 0 || mission._flags.state == MISSION_COMPLETE) {
        mission._prev_nav_cmd_id    = AP_MISSION_CMD_ID_NONE;
        mission._prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
        mission._prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
        // reset the jump tracking to zero
        mission_init_jump_tracking();
        if (index == 0) {
            index = 1;
        }
    }

    // if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped
    // so that if the user resumes the mission it will begin at the specified index
    if (mission._flags.state != MISSION_RUNNING) {
        // search until we find next nav command or reach end of command list
        while (!mission._flags.nav_cmd_loaded) {
            // get next command
            if (!mission_get_next_cmd(index, &cmd, true, true)) {
                mission._nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
                return false;
            }

            // check if navigation or "do" command
            if (mission_is_nav_cmd(&cmd)) {
                // set current navigation command
                mission._nav_cmd = cmd;
                mission._flags.nav_cmd_loaded = true;
            } else {
                // set current do command
                if (!mission._flags.do_cmd_loaded) {
                    mission._do_cmd = cmd;
                    mission._flags.do_cmd_loaded = true;
                }
            }
            // move onto next command
            index = cmd.index+1;
        }

        // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
        if (!mission._flags.do_cmd_loaded) {
            mission._flags.do_cmd_all_done = true;
        }

        // if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped"
        mission._flags.state = MISSION_STOPPED;
        return true;
    }

    // the state must be MISSION_RUNNING, allow advance_current_nav_cmd() to manage starting the item
    if (!mission_advance_current_nav_cmd(index)) {
        // on failure set mission complete
        mission_complete();
        return false;
    }

    // if we got this far we must have successfully advanced the nav command
    return true;
}

// restart current navigation command.  Used to handle external changes to mission
// returns true on success, false if mission is not running or current nav command is invalid
bool mission_restart_current_nav_cmd()
{
    // return immediately if mission is not running
    if (mission._flags.state != MISSION_RUNNING) {
        return false;
    }

    // return immediately if nav command index is invalid
    const uint16_t nav_cmd_index = mission_get_current_nav_index();
    if ((nav_cmd_index == 0) || (nav_cmd_index >= mission_num_commands())) {
        return false;
    }

    return mission_set_current_cmd(mission._nav_cmd.index, false);
}

/// num_commands - returns total number of commands in the mission
///                 this number includes offset 0, the home location
uint16_t mission_num_commands()
{
    return mission._cmd_total;
}

// return the last time the mission changed in milliseconds
uint32_t mission_last_change_time_ms(void)
{
    return mission._last_change_time_ms;
}

/// get_current_nav_index - returns the current "navigation" command index
/// Note that this will return 0 if there is no command. This is
/// used in MAVLink reporting of the mission command
uint16_t mission_get_current_nav_index()
{
    return mission._nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:mission._nav_cmd.index;
}

/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
///     returns true if found, false if not found (i.e. reached end of mission command list)
///     accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
bool mission_get_next_nav_cmd(uint16_t start_index, mission_command_t cmd)
{
    // search until the end of the mission command list
    for (uint16_t cmd_index = start_index; cmd_index < (unsigned)mission._cmd_total; cmd_index++) {
        // get next command
        if (!mission_get_next_cmd(cmd_index, cmd, false, true)) {
            // no more commands so return failure
            return false;
        }
        // if found a "navigation" command then return it
        if (mission_is_nav_cmd(cmd)) {
            return true;
        }
    }

    // if we got this far we did not find a navigation command
    return false;
}

/// add_cmd - adds a command to the end of the command list and writes to storage
///     returns true if successfully added, false on failure
///     cmd.index is updated with it's new position in the mission
bool mission_add_cmd(mission_command_t cmd)
{
    // attempt to write the command to storage
    bool ret = mission_write_cmd_to_storage(mission._cmd_total, cmd);

    if (ret) {
        // update command's index
        cmd->index = mission._cmd_total;
        // increment total number of commands
        mission_param_set_and_save("MIS_TOTAL", mission._cmd_total + 1);
    }

    return ret;
}

/// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd
///     replacing the current active command will have no effect until the command is restarted
///     returns true if successfully replaced, false on failure
bool mission_replace_cmd(uint16_t index, const mission_command_t cmd)
{
    // sanity check index
    if (index >= (unsigned)mission._cmd_total) {
        return false;
    }

    // attempt to write the command to storage
    return mission_write_cmd_to_storage(index, cmd);
}

/// get the ground course of the next navigation leg in centidegrees
/// from 0 36000. Return default_angle if next navigation
/// leg cannot be determined
int32_t mission_get_next_ground_course_cd(int32_t default_angle)
{
    struct Mission_Command cmd;
    if (!mission_get_next_nav_cmd(mission._nav_cmd.index+1, &cmd)) {
        return default_angle;
    }
    // special handling for nav commands with no target location
    if (cmd.id == MAV_CMD_NAV_GUIDED_ENABLE ||
        cmd.id == MAV_CMD_NAV_DELAY) {
        return default_angle;
    }
    if (cmd.id == MAV_CMD_NAV_SET_YAW_SPEED) {
        return (mission._nav_cmd.content.set_yaw_speed.angle_deg * 100);
    }
    return location_get_bearing_to(&mission._nav_cmd.content.location, &cmd.content.location);
}

// returns false on any issue at all.
bool mission_set_item(uint16_t index, mavlink_mission_item_int_t* src_packet)
{
    // this is the on-storage format
    struct Mission_Command cmd;

    // can't handle request for anything bigger than the mission size+1...
    if (index > mission_num_commands()) {
        return false;
    }

    // convert from mavlink-ish format to storage format, if we can.
    if (mission_mavlink_int_to_mission_cmd(src_packet, &cmd) != MAV_MISSION_ACCEPTED) {
        return false;
    }

    // A request to set the 'next' item after the end is how we add an extra
    //  item to the list, thus allowing us to write entire missions if needed.
    if (index == mission_num_commands()) {
        return mission_add_cmd(&cmd);
    }

    // replacing an existing mission item...
    return mission_replace_cmd(index, &cmd);
}

bool mission_get_item(uint16_t index, mavlink_mission_item_int_t* ret_packet)
{
    // setting ret_packet.command = -1  and/or returning false
    //  means it contains invalid data after it leaves here.

    // this is the on-storage format
    struct Mission_Command cmd;

    // can't handle request for anything bigger than the mission size...
    if (index >= mission_num_commands()) {
        ret_packet->command = -1;
        return false;
    }

    // minimal placeholder values during read-from-storage
    ret_packet->target_system = 1;     // unused sysid
    ret_packet->target_component =  1; // unused compid

    // 0=home, higher number/s = mission item number.
    ret_packet->seq = index;

    // retrieve mission from eeprom
    if (!mission_read_cmd_from_storage(ret_packet->seq, &cmd)) {
        ret_packet->command = -1;
        return false;
    }
    // convert into mavlink-ish format for lua and friends.
    if (!mission_mission_cmd_to_mavlink_int(&cmd, ret_packet)) {
        ret_packet->command = -1;
        return false;
    }

    // set packet's current field to 1 if this is the command being executed
    if (cmd.id == (uint16_t)mission._nav_cmd.index) {
        ret_packet->current = 1;
    } else {
        ret_packet->current = 0;
    }

    // set auto continue to 1, becasue that's what's done elsewhere.
    ret_packet->autocontinue = 1;     // 1 (true), 0 (false)
    ret_packet->command = cmd.id;

    return true;
}

/// get_current_nav_cmd - returns the current "navigation" command
const mission_command_t mission_get_current_nav_cmd()
{
    return &mission._nav_cmd;
}

// find the nearest landing sequence starting point (DO_LAND_START) and
// return its index.  Returns 0 if no appropriate DO_LAND_START point can
// be found.
uint16_t mission_get_landing_sequence_start()
{
    Location current_loc;

    if (!ahrs_get_location(get_ahrs_view(), &current_loc)) {
        return 0;
    }

    const AltFrame current_loc_alt_frame = location_get_alt_frame(&current_loc);
    uint16_t landing_start_index = 0;
    float min_distance = -1;
    Vector3f_t distance_3f = VECTOR3F_DEFAULT_VALUE;

    // Go through mission looking for nearest landing start command
    for (uint16_t i = 1; i < mission._cmd_total; i++) {
        struct Mission_Command tmp;
        if (!mission_read_cmd_from_storage(i, &tmp)) {
            continue;
        }
        if (tmp.id == MAV_CMD_DO_LAND_START) {
            if (!location_initialised(&tmp.content.location) && !mission_get_next_nav_cmd(i, &tmp)) {
                // command does not have a valid location and cannot get next valid
                continue;
            }

            float tmp_distance;
            if (current_loc_alt_frame == location_get_alt_frame(&tmp.content.location) || location_change_alt_frame(&tmp.content.location, current_loc_alt_frame)) {
                // 3D distance - altitudes are able to be compared in the same frame
                distance_3f = location_get_distance_ned(&tmp.content.location , &current_loc);
                tmp_distance = vec3_length(&distance_3f);
            } else {
                // 2D distance - no altitude
                tmp_distance = location_get_distance(&tmp.content.location, &current_loc);
            }

            if (min_distance < 0 || tmp_distance < min_distance) {
                min_distance = tmp_distance;
                landing_start_index = i;
            }
        }
    }

    return landing_start_index;
}

/*
   find the nearest landing sequence starting point (DO_LAND_START) and
   switch to that mission item.  Returns false if no DO_LAND_START
   available.
 */
bool mission_jump_to_landing_sequence(void)
{
    uint16_t land_idx = mission_get_landing_sequence_start();
    if (land_idx != 0 && mission_set_current_cmd(land_idx, false)) {

        //if the mission has ended it has to be restarted
        if (mission._flags.state == MISSION_STOPPED) {
            mission_resume();
        }

        gcs_send_text(MAV_SEVERITY_INFO, "Landing sequence start");
        mission._flags.in_landing_sequence = true;
        return true;
    }

    gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
    return false;
}

// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
bool mission_jump_to_abort_landing_sequence(void)
{
    Location current_loc;

    uint16_t abort_index = 0;
    if (ahrs_get_location(get_ahrs_view(), &current_loc)) {
        float min_distance = FLT_MAX;

        for (uint16_t i = 1; i < mission._cmd_total; i++) {
            struct Mission_Command tmp;
            if (!mission_read_cmd_from_storage(i, &tmp)) {
                continue;
            }
            if (tmp.id == MAV_CMD_DO_GO_AROUND) {
                float tmp_distance = location_get_distance(&tmp.content.location, &current_loc);
                if (tmp_distance < min_distance) {
                    min_distance = tmp_distance;
                    abort_index = i;
                }
            }
        }
    }

    if (abort_index != 0 && mission_set_current_cmd(abort_index, false)) {

        //if the mission has ended it has to be restarted
        if (mission._flags.state == MISSION_STOPPED) {
            mission_resume();
        }

        mission._flags.in_landing_sequence = false;

        gcs_send_text(MAV_SEVERITY_INFO, "Landing abort sequence start");
        return true;
    }

    gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
    return false;
}

// check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
bool mission_is_best_land_sequence(void)
{
    // check if there is even a running mission to interupt
    if (mission._flags.state != MISSION_RUNNING) {
        return false;
    }

    // check if aircraft has already jumped to a landing sequence
    if (mission._flags.in_landing_sequence) {
        return true;
    }

    // check if MIS_OPTIONS bit set to allow distance calculation to be done
    if (!(mission._options & AP_MISSION_MASK_DIST_TO_LAND_CALC)) {
        return false;
    }

    // The decision to allow a failsafe to interupt a potential landing approach
    // is a distance travelled minimization problem.  Look forward in
    // mission to evaluate the shortest remaining distance to land.

    // go through the mission for the nearest DO_LAND_START first as this is the most probable route
    // to a landing with the minimum number of WP.
    uint16_t do_land_start_index = mission_get_landing_sequence_start();
    if (do_land_start_index == 0) {
        // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
        return false;
    }

    // get our current location
    Location current_loc;
    if (!ahrs_get_location(get_ahrs_view(), &current_loc)) {
        // we don't know where we are!!
        return false;
    }

    // get distance to landing if travelled to nearest DO_LAND_START via RTL
    float dist_via_do_land;
    if (!mission_distance_to_landing(do_land_start_index, &dist_via_do_land, current_loc)) {
        // cant get a valid distance to landing
        return false;
    }

    // get distance to landing if continue along current mission path
    float dist_continue_to_land;
    if (!mission_distance_to_landing(mission._nav_cmd.index, &dist_continue_to_land, current_loc)) {
        // cant get a valid distance to landing
        return false;
    }

    // compare distances
    if (dist_via_do_land >= dist_continue_to_land) {
        // then the mission should carry on uninterrupted as that is the shorter distance
        gcs_send_text(MAV_SEVERITY_NOTICE, "Rejecting RTL: closer land if mis continued");
        return true;
    } else {
        // allow failsafes to interrupt the current mission
        return false;
    }
}

bool mission_contains_item(MAV_CMD command)
{
    for (int i = 1; i < mission._cmd_total; i++) {
        struct Mission_Command tmp;
        if (!mission_read_cmd_from_storage(i, &tmp)) {
            continue;
        }
        if (tmp.id == command) {
            return true;
        }
    }
    return false;
}

/*
  return true if the mission has a terrain relative item.  ~2200us for 530 items on H7
 */
bool mission_contains_terrain_alt_items(void)
{
    if (mission._last_contains_relative_calculated_ms != mission._last_change_time_ms) {
        mission._contains_terrain_alt_items = mission_calculate_contains_terrain_alt_items();
        mission._last_contains_relative_calculated_ms = mission._last_change_time_ms;
    }
    return mission._contains_terrain_alt_items;
}

MAV_MISSION_RESULT mission_SanityCheckParams(const mavlink_mission_item_int_t* packet)
{
    uint8_t nan_mask = 0;
    switch (packet->command) {
    case MAV_CMD_NAV_WAYPOINT:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_LAND:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_LAND:
        nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan
        break;
    default:
        nan_mask = 0xff;
        break;
    }

    if (((nan_mask & (1 << 0)) && isnan(packet->param1)) ||
        isinf(packet->param1)) {
        return MAV_MISSION_INVALID_PARAM1;
    }
    if (((nan_mask & (1 << 1)) && isnan(packet->param2)) ||
        isinf(packet->param2)) {
        return MAV_MISSION_INVALID_PARAM2;
    }
    if (((nan_mask & (1 << 2)) && isnan(packet->param3)) ||
        isinf(packet->param3)) {
        return MAV_MISSION_INVALID_PARAM3;
    }
    if (((nan_mask & (1 << 3)) && isnan(packet->param4)) ||
        isinf(packet->param4)) {
        return MAV_MISSION_INVALID_PARAM4;
    }
    return MAV_MISSION_ACCEPTED;
}

// mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
//  return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t* packet, mission_command_t cmd)
{
    // command's position in mission list and mavlink id
    cmd->index = packet->seq;
    cmd->id = packet->command;
    memset(&(cmd->content.location),0,sizeof(cmd->content.location));

    MAV_MISSION_RESULT param_check = mission_SanityCheckParams(packet);
    if (param_check != MAV_MISSION_ACCEPTED) {
        return param_check;
    }

    // command specific conversions from mavlink packet to mission command
    switch (cmd->id) {

    case 0:
        // this is reserved for storing 16 bit command IDs
        return MAV_MISSION_INVALID;

    case MAV_CMD_NAV_WAYPOINT: {                        // MAV ID: 16
        /*
          the 15 byte limit means we can't fit both delay and radius
          in the cmd structure. When we expand the mission structure
          we can do this properly
         */
        // delay at waypoint in seconds (this is for copters???)
        cmd->p1 = packet->param1;
    }
    break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        cmd->p1 = fabsf(packet->param3);                  // store radius as 16bit since no other params are competing for space
        cmd->content.location.loiter_ccw = (packet->param3 < 0);    // -1 = counter clockwise, +1 = clockwise
        break;

    case MAV_CMD_NAV_LOITER_TURNS: {                    // MAV ID: 18
        uint16_t num_turns = packet->param1;              // param 1 is number of times to circle is held in low p1
        uint16_t radius_m = fabsf(packet->param3);        // param 3 is radius in meters is held in high p1
        cmd->p1 = (radius_m<<8) | (num_turns & 0x00FF);   // store radius in high byte of p1, num turns in low byte of p1
        cmd->content.location.loiter_ccw = (packet->param3 < 0);
        cmd->content.location.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
    }
    break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        cmd->p1 = packet->param1;                         // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius.
        cmd->content.location.loiter_ccw = (packet->param3 < 0);
        cmd->content.location.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        cmd->p1 = packet->param1;                         // abort target altitude(m)  (plane only)
        cmd->content.location.loiter_ccw = math_flt_negative(packet->param4); // yaw direction, (plane deepstall only)
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        cmd->p1 = packet->param1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        cmd->p1 = packet->param1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        cmd->p1 = fabsf(packet->param2);                  // param2 is radius in meters
        cmd->content.location.loiter_ccw = (packet->param2 < 0);
        cmd->content.location.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
        cmd->p1 = packet->param1;                         // delay at waypoint in seconds
        break;

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        cmd->p1 = packet->param1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        cmd->content.nav_delay.seconds = packet->param1; // delay in seconds
        cmd->content.nav_delay.hour_utc = packet->param2;// absolute time's hour (utc)
        cmd->content.nav_delay.min_utc = packet->param3;// absolute time's min (utc)
        cmd->content.nav_delay.sec_utc = packet->param4; // absolute time's second (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        cmd->content.delay.seconds = packet->param1;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        cmd->content.distance.meters = packet->param1;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        cmd->content.yaw.angle_deg = packet->param1;      // target angle in degrees
        cmd->content.yaw.turn_rate_dps = packet->param2;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        cmd->content.yaw.direction = packet->param3;      // -1 = ccw, +1 = cw
        cmd->content.yaw.relative_angle = packet->param4; // lng=0: absolute angle provided, lng=1: relative angle provided
        break;

    case MAV_CMD_DO_SET_MODE:                           // MAV ID: 176
        cmd->p1 = packet->param1;                         // flight mode identifier
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
        cmd->content.jump.target = packet->param1;        // jump-to command number
        cmd->content.jump.num_times = packet->param2;     // repeat count
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        cmd->content.speed.speed_type = packet->param1;   // 0 = airspeed, 1 = ground speed
        cmd->content.speed.target_ms = packet->param2;    // target speed in m/s
        cmd->content.speed.throttle_pct = packet->param3; // throttle as a percentage from 0 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:
        cmd->p1 = packet->param1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_LAND_START:                         // MAV ID: 189
        break;

    case MAV_CMD_DO_GO_AROUND:                          // MAV ID: 191
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        cmd->p1 = packet->param1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        cmd->p1 = packet->param1;                         // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_PARACHUTE:                         // MAV ID: 208
        cmd->p1 = packet->param1;                        // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        cmd->p1 = packet->param1;                         // normal=0 inverted=1
        break;

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        cmd->p1 = packet->param1;                         // max time in seconds the external controller will be allowed to control the vehicle
        cmd->content.guided_limits.alt_min = packet->param2;  // min alt below which the command will be aborted.  0 for no lower alt limit
        cmd->content.guided_limits.alt_max = packet->param3;  // max alt above which the command will be aborted.  0 for no upper alt limit
        cmd->content.guided_limits.horiz_max = packet->param4;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_NAV_VTOL_TAKEOFF:
        break;

    case MAV_CMD_NAV_VTOL_LAND:
        break;

    case MAV_CMD_NAV_SET_YAW_SPEED:
        cmd->content.set_yaw_speed.angle_deg = packet->param1;        // target angle in degrees
        cmd->content.set_yaw_speed.speed = packet->param2;            // speed in meters/second
        cmd->content.set_yaw_speed.relative_angle = packet->param3;   // 0 = absolute angle, 1 = relative angle
        break;
    default:
        // unrecognised command
        return MAV_MISSION_UNSUPPORTED;
    }

    // copy location from mavlink to command
    if (mission_stored_in_location(cmd->id)) {

        // sanity check location
        if (!location_check_lat(packet->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(packet->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        if (isnan(packet->z) || fabsf(packet->z) >= LOCATION_ALT_MAX_M) {
            return MAV_MISSION_INVALID_PARAM7;
        }

        cmd->content.location.lat = packet->x;
        cmd->content.location.lng = packet->y;

        cmd->content.location.alt = packet->z * 100.0f;       // convert packet's alt (m) to cmd alt (cm)

        switch (packet->frame) {

        case MAV_FRAME_MISSION:
        case MAV_FRAME_GLOBAL:
        case MAV_FRAME_GLOBAL_INT:
            cmd->content.location.relative_alt = 0;
            break;

        case MAV_FRAME_GLOBAL_RELATIVE_ALT:
        case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
            cmd->content.location.relative_alt = 1;
            break;
        default:
            return MAV_MISSION_UNSUPPORTED_FRAME;
        }
    }

    // if we got this far then it must have been successful
    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t *packet,
        mavlink_mission_item_int_t *mav_cmd)
{
    // TODO: rename mav_cmd to mission_item_int
    // TODO: rename packet to mission_item
    mav_cmd->param1 = packet->param1;
    mav_cmd->param2 = packet->param2;
    mav_cmd->param3 = packet->param3;
    mav_cmd->param4 = packet->param4;
    mav_cmd->z = packet->z;
    mav_cmd->seq = packet->seq;
    mav_cmd->command = packet->command;
    mav_cmd->target_system = packet->target_system;
    mav_cmd->target_component = packet->target_component;
    mav_cmd->frame = packet->frame;
    mav_cmd->current = packet->current;
    mav_cmd->autocontinue = packet->autocontinue;

    /*
      the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT
      is to pass the lat/lng in MISSION_ITEM_INT straight through, and
      for MISSION_ITEM multiply by 1e7 here. We need an exception for
      any commands which use the x and y fields not as
      latitude/longitude.
     */
    switch (packet->command) {
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        mav_cmd->x = packet->x;
        mav_cmd->y = packet->y;
        break;

    default:
        // all other commands use x and y as lat/lon. We need to
        // multiply by 1e7 to convert to int32_t
        if (!location_check_lat(packet->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(packet->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        mav_cmd->x = packet->x * 1.0e7f;
        mav_cmd->y = packet->y * 1.0e7f;
        break;
    }

    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t *item_int,
        mavlink_mission_item_t *item)
{
    item->param1 = item_int->param1;
    item->param2 = item_int->param2;
    item->param3 = item_int->param3;
    item->param4 = item_int->param4;
    item->z = item_int->z;
    item->seq = item_int->seq;
    item->command = item_int->command;
    item->target_system = item_int->target_system;
    item->target_component = item_int->target_component;
    item->frame = item_int->frame;
    item->current = item_int->current;
    item->autocontinue = item_int->autocontinue;

    switch (item_int->command) {
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        item->x = item_int->x;
        item->y = item_int->y;
        break;

    default:
        // all other commands use x and y as lat/lon. We need to
        // multiply by 1e-7 to convert to float
        item->x = item_int->x * 1.0e-7f;
        item->y = item_int->y * 1.0e-7f;
        if (!location_check_lat(item->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(item->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        break;
    }

    return MAV_MISSION_ACCEPTED;
}

// mavlink_cmd_long_to_mission_cmd - converts a mavlink cmd long to an AP_Mission::Mission_Command object which can be stored to eeprom
// return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_mavlink_cmd_long_to_mission_cmd(const mavlink_command_long_t* packet, mission_command_t cmd)
{
    mavlink_mission_item_int_t miss_item = {0};

    miss_item.param1 = packet->param1;
    miss_item.param2 = packet->param2;
    miss_item.param3 = packet->param3;
    miss_item.param4 = packet->param4;

    miss_item.command = packet->command;
    miss_item.target_system = packet->target_system;
    miss_item.target_component = packet->target_component;

    return mission_mavlink_int_to_mission_cmd(&miss_item, cmd);
}

// mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
//  return true on success, false on failure
bool mission_mission_cmd_to_mavlink_int(const mission_command_t cmd, mavlink_mission_item_int_t* packet)
{
    // command's position in mission list and mavlink id
    packet->seq = cmd->index;
    packet->command = cmd->id;

    // set defaults
    packet->current = 0;     // 1 if we are passing back the mission command that is currently being executed
    packet->param1 = 0;
    packet->param2 = 0;
    packet->param3 = 0;
    packet->param4 = 0;
    packet->autocontinue = 1;

    // command specific conversions from mission command to mavlink packet
    switch (cmd->id) {
    case 0:
        // this is reserved for 16 bit command IDs
        return false;

    case MAV_CMD_NAV_WAYPOINT:                          // MAV ID: 16
        // delay at waypoint in seconds
        packet->param1 = cmd->p1;
        break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        packet->param3 = (float)cmd->p1;
        if (cmd->content.location.loiter_ccw) {
            packet->param3 *= -1;
        }
        break;

    case MAV_CMD_NAV_LOITER_TURNS:                      // MAV ID: 18
        packet->param1 = LOWBYTE(cmd->p1);                // number of times to circle is held in low byte of p1
        packet->param3 = HIGHBYTE(cmd->p1);               // radius is held in high byte of p1
        if (cmd->content.location.loiter_ccw) {
            packet->param3 = -packet->param3;
        }
        packet->param4 = cmd->content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        packet->param1 = cmd->p1;                         // loiter time in seconds
        if (cmd->content.location.loiter_ccw) {
            packet->param3 = -1;
        } else {
            packet->param3 = 1;
        }
        packet->param4 = cmd->content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        packet->param1 = cmd->p1;                        // abort target altitude(m)  (plane only)
        packet->param4 = cmd->content.location.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only)
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        packet->param1 = cmd->p1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        packet->param1 = cmd->p1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        packet->param2 = cmd->p1;                        // loiter radius(m)
        if (cmd->content.location.loiter_ccw) {
            packet->param2 = -packet->param2;
        }
        packet->param4 = cmd->content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
        packet->param1 = cmd->p1;                         // delay at waypoint in seconds
        break;

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        packet->param1 = cmd->p1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        packet->param1 = cmd->content.nav_delay.seconds; // delay in seconds
        packet->param2 = cmd->content.nav_delay.hour_utc; // absolute time's day of week (utc)
        packet->param3 = cmd->content.nav_delay.min_utc; // absolute time's hour (utc)
        packet->param4 = cmd->content.nav_delay.sec_utc; // absolute time's min (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        packet->param1 = cmd->content.delay.seconds;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        packet->param1 = cmd->content.distance.meters;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        packet->param1 = cmd->content.yaw.angle_deg;      // target angle in degrees
        packet->param2 = cmd->content.yaw.turn_rate_dps;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        packet->param3 = cmd->content.yaw.direction;      // -1 = ccw, +1 = cw
        packet->param4 = cmd->content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
        break;

    case MAV_CMD_DO_SET_MODE:                           // MAV ID: 176
        packet->param1 = cmd->p1;                         // set flight mode identifier
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
        packet->param1 = cmd->content.jump.target;        // jump-to command number
        packet->param2 = cmd->content.jump.num_times;     // repeat count
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        packet->param1 = cmd->content.speed.speed_type;   // 0 = airspeed, 1 = ground speed
        packet->param2 = cmd->content.speed.target_ms;    // speed in m/s
        packet->param3 = cmd->content.speed.throttle_pct; // throttle as a percentage from 0 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:                           // MAV ID: 179
        packet->param1 = cmd->p1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        packet->param1 = cmd->p1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        packet->param1 = cmd->p1;                         // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_PARACHUTE:                          // MAV ID: 208
        packet->param1 = cmd->p1;                         // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        packet->param1 = cmd->p1;                         // normal=0 inverted=1
        break;

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        packet->param1 = cmd->p1;                         // max time in seconds the external controller will be allowed to control the vehicle
        packet->param2 = cmd->content.guided_limits.alt_min;  // min alt below which the command will be aborted.  0 for no lower alt limit
        packet->param3 = cmd->content.guided_limits.alt_max;  // max alt above which the command will be aborted.  0 for no upper alt limit
        packet->param4 = cmd->content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_DO_SET_REVERSE:
        packet->param1 = cmd->p1;   // 0 = forward, 1 = reverse
        break;

    case MAV_CMD_DO_ENGINE_CONTROL:
        packet->param1 = cmd->content.do_engine_control.start_control?1:0;
        packet->param2 = cmd->content.do_engine_control.cold_start?1:0;
        packet->param3 = cmd->content.do_engine_control.height_delay_cm*0.01f;
        break;

    case MAV_CMD_NAV_PAYLOAD_PLACE:
        packet->param1 = cmd->p1*.001f; // copy max-descend parameter (m->cm)
        break;

    case MAV_CMD_NAV_SET_YAW_SPEED:
        packet->param1 = cmd->content.set_yaw_speed.angle_deg;        // target angle in degrees
        packet->param2 = cmd->content.set_yaw_speed.speed;            // speed in meters/second
        packet->param3 = cmd->content.set_yaw_speed.relative_angle;   // 0 = absolute angle, 1 = relative angle
        break;
    default:
        // unrecognised command
        return false;
    }

    // copy location from mavlink to command
    if (mission_stored_in_location(cmd->id)) {
        packet->x = cmd->content.location.lat;
        packet->y = cmd->content.location.lng;

        packet->z = cmd->content.location.alt * 0.01f;   // cmd alt in cm to m
        if (cmd->content.location.relative_alt) {
            packet->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
        } else {
            packet->frame = MAV_FRAME_GLOBAL;
        }
        // don't ever return terrain mission items if no terrain support
        if (cmd->content.location.terrain_alt) {
            return false;
       }
    }

    // if we got this far then it must have been successful
    return true;
}

const char *mission_command_type(const mission_command_t cmd)
{
    switch (cmd->id) {
    case MAV_CMD_NAV_WAYPOINT:
        return "WP";
    case MAV_CMD_NAV_SPLINE_WAYPOINT:
        return "SplineWP";
    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
        return "RTL";
    case MAV_CMD_NAV_LOITER_UNLIM:
        return "LoitUnlim";
    case MAV_CMD_NAV_LOITER_TIME:
        return "LoitTime";
    case MAV_CMD_NAV_GUIDED_ENABLE:
        return "GuidedEnable";
    case MAV_CMD_NAV_LOITER_TURNS:
        return "LoitTurns";
    case MAV_CMD_NAV_LOITER_TO_ALT:
        return "LoitAltitude";
    case MAV_CMD_NAV_SET_YAW_SPEED:
        return "SetYawSpd";
    case MAV_CMD_CONDITION_DELAY:
        return "CondDelay";
    case MAV_CMD_CONDITION_DISTANCE:
        return "CondDist";
    case MAV_CMD_DO_CHANGE_SPEED:
        return "ChangeSpeed";
    case MAV_CMD_DO_SET_HOME:
        return "SetHome";
    case MAV_CMD_DO_SET_SERVO:
        return "SetServo";
    case MAV_CMD_DO_SET_RELAY:
        return "SetRelay";
    case MAV_CMD_DO_REPEAT_SERVO:
        return "RepeatServo";
    case MAV_CMD_DO_REPEAT_RELAY:
        return "RepeatRelay";
    case MAV_CMD_DO_CONTROL_VIDEO:
        return "CtrlVideo";
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        return "DigiCamCfg";
    case MAV_CMD_DO_DIGICAM_CONTROL:
        return "DigiCamCtrl";
    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
        return "SetCamTrigDst";
    case MAV_CMD_DO_SET_ROI:
        return "SetROI";
    case MAV_CMD_DO_SET_REVERSE:
        return "SetReverse";
    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
        return "SetRepeatDist";
    case MAV_CMD_DO_GUIDED_LIMITS:
        return "GuidedLimits";
    case MAV_CMD_NAV_TAKEOFF:
        return "Takeoff";
    case MAV_CMD_NAV_LAND:
        return "Land";
    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
        return "ContinueAndChangeAlt";
    case MAV_CMD_NAV_ALTITUDE_WAIT:
        return "AltitudeWait";
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        return "VTOLTakeoff";
    case MAV_CMD_NAV_VTOL_LAND:
        return "VTOLLand";
    case MAV_CMD_DO_INVERTED_FLIGHT:
        return "InvertedFlight";
    case MAV_CMD_DO_FENCE_ENABLE:
        return "FenceEnable";
    case MAV_CMD_DO_AUTOTUNE_ENABLE:
        return "AutoTuneEnable";
    case MAV_CMD_DO_VTOL_TRANSITION:
        return "VTOLTransition";
    case MAV_CMD_DO_ENGINE_CONTROL:
        return "EngineControl";
    case MAV_CMD_CONDITION_YAW:
        return "CondYaw";
    case MAV_CMD_DO_LAND_START:
        return "LandStart";
    case MAV_CMD_NAV_DELAY:
        return "Delay";
    case MAV_CMD_DO_GRIPPER:
        return "Gripper";
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        return "PayloadPlace";
    case MAV_CMD_DO_PARACHUTE:
        return "Parachute";
    case MAV_CMD_DO_SPRAYER:
        return "Sprayer";
    case MAV_CMD_DO_AUX_FUNCTION:
        return "AuxFunction";
    case MAV_CMD_DO_MOUNT_CONTROL:
        return "MountControl";
    case MAV_CMD_DO_WINCH:
        return "Winch";
    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
        return "Scripting";
    case MAV_CMD_DO_JUMP:
        return "Jump";
    case MAV_CMD_DO_GO_AROUND:
        return "Go Around";
    case MAV_CMD_NAV_SCRIPT_TIME:
        return "NavScriptTime";
    case MAV_CMD_NAV_ATTITUDE_TIME:
        return "NavAttitudeTime";
    case MAV_CMD_DO_PAUSE_CONTINUE:
        return "PauseContinue";
    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        return "GimbalPitchYaw";
    default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        console_panic("Mission command with ID %u has no string", cmd->id);
#endif
        return "?";
    }
}

///
/// private methods
///

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void mission_assign_param()
{
    param_link_variable(PARAM_ID(MIS, MIS_TOTAL), &mission._cmd_total);
    param_link_variable(PARAM_ID(MIS, MIS_RESTART), &mission._restart);
    param_link_variable(PARAM_ID(MIS, MIS_OPTIONS), &mission._options);
}

// check_eeprom_version - checks version of missions stored in eeprom matches this library
// command list will be cleared if they do not match
static void mission_check_eeprom_version()
{
    uint32_t eeprom_version = StorageAccess_read_uint32(&mission._storage, 0);

    // if eeprom version does not match, clear the command list and update the eeprom version
    if (eeprom_version != AP_MISSION_EEPROM_VERSION) {
        if (mission_clear()) {
            StorageAccess_write_uint32(&mission._storage, 0, AP_MISSION_EEPROM_VERSION);
        }
    }
}

// 
static bool mission_stored_in_location(uint16_t id)
{
    switch (id) {
    case MAV_CMD_NAV_WAYPOINT:
    case MAV_CMD_NAV_LOITER_UNLIM:
    case MAV_CMD_NAV_LOITER_TURNS:
    case MAV_CMD_NAV_LOITER_TIME:
    case MAV_CMD_NAV_LAND:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
    case MAV_CMD_NAV_LOITER_TO_ALT:
    case MAV_CMD_NAV_SPLINE_WAYPOINT:
    case MAV_CMD_NAV_GUIDED_ENABLE:
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_LAND_START:
    case MAV_CMD_DO_GO_AROUND:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        return true;
    default:
        return false;
    }
}

static void mission_param_set_and_save(const char* param_name, double value)
{
    param_t* p = NULL;

    for(int i= 0; i < ARRAY_SIZE(var_info); i++){
        if(strcmp(param_name, var_info[i].name) == 0) {
            p = &var_info[i];
            break;
        }
    }

    if (p != NULL) {
        param_set_and_save(p, value);
    }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    else {
        printf("[mission]: The parameter to be set does not exist!\n");
    }
#endif
}

// init_jump_tracking - initialise jump_tracking variables
static void mission_init_jump_tracking()
{
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        mission._jump_tracking[i].index = AP_MISSION_CMD_INDEX_NONE;
        mission._jump_tracking[i].num_times_run = 0;
    }
}

/// get_jump_times_run - returns number of times the jump command has been run
static int16_t mission_get_jump_times_run(const mission_command_t cmd)
{
    // exit immediately if cmd is not a do-jump command or target is invalid
    if ((cmd->id != MAV_CMD_DO_JUMP) || (cmd->content.jump.target >= (unsigned)mission._cmd_total) || (cmd->content.jump.target == 0)) {
        // To-Do: log an error?
        return AP_MISSION_JUMP_TIMES_MAX;
    }

    // search through jump_tracking array for this cmd
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        if (mission._jump_tracking[i].index == cmd->index) {
            return mission._jump_tracking[i].num_times_run;
        } else if (mission._jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
            // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
            mission._jump_tracking[i].index = cmd->index;
            mission._jump_tracking[i].num_times_run = 0;
            return 0;
        }
    }

    // if we've gotten this far then the _jump_tracking array must be full
    // To-Do: log an error?
    return AP_MISSION_JUMP_TIMES_MAX;
}

/// increment_jump_times_run - increments the recorded number of times the jump command has been run
static void mission_increment_jump_times_run(mission_command_t cmd, bool send_gcs_msg)
{
    // exit immediately if cmd is not a do-jump command
    if (cmd->id != MAV_CMD_DO_JUMP) {
        // To-Do: log an error?
        return;
    }

    // search through jump_tracking array for this cmd
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        if (mission._jump_tracking[i].index == cmd->index) {
            mission._jump_tracking[i].num_times_run++;
            if (send_gcs_msg) {
                gcs_send_text(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", mission._jump_tracking[i].index, mission._jump_tracking[i].num_times_run, cmd->content.jump.num_times);
            }
            return;
        } else if (mission._jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
            // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
            mission._jump_tracking[i].index = cmd->index;
            mission._jump_tracking[i].num_times_run = 1;
            return;
        }
    }

    // if we've gotten this far then the _jump_tracking array must be full
    // To-Do: log an error
    return;
}

/// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn
static void mission_complete()
{
    // flag mission as complete
    mission._flags.state = MISSION_COMPLETE;
    mission._flags.in_landing_sequence = false;

    // callback to main program's mission complete function
    mission._mission_complete_fn();
}

/// advance_current_nav_cmd - moves current nav command forward
///     do command will also be loaded
///     accounts for do-jump commands
//      returns true if command is advanced, false if failed (i.e. mission completed)
//  uint16_t starting_index = 0
static bool mission_advance_current_nav_cmd(uint16_t starting_index)
{
    // exit immediately if we're not running
    if (mission._flags.state != MISSION_RUNNING) {
        return false;
    }

    // exit immediately if current nav command has not completed
    if (mission._flags.nav_cmd_loaded) {
        return false;
    }

    // stop the current running do command
    mission._do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
    mission._flags.do_cmd_loaded = false;
    mission._flags.do_cmd_all_done = false;

    // get starting point for search
    uint16_t cmd_index = starting_index > 0 ? starting_index - 1 : mission._nav_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        // start from beginning of the mission command list
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    } else {
        // start from one position past the current nav command
        cmd_index++;
    }

    // avoid endless loops
    uint8_t max_loops = 255;

    // search until we find next nav command or reach end of command list
    while (!mission._flags.nav_cmd_loaded && max_loops-- > 0) {
        // get next command
        struct Mission_Command cmd;
        if (!mission_get_next_cmd(cmd_index, &cmd, true, true)) {
            return false;
        }

        // check if navigation or "do" command
        if (mission_is_nav_cmd(&cmd)) {
            // save previous nav command index
            mission._prev_nav_cmd_id = mission._nav_cmd.id;
            mission._prev_nav_cmd_index = mission._nav_cmd.index;
            // save separate previous nav command index if it contains lat,long,alt
            if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
                mission._prev_nav_cmd_wp_index = mission._nav_cmd.index;
            }
            // set current navigation command and start it
            mission._nav_cmd = cmd;
            if (mission_start_command(&mission._nav_cmd)) {
                mission._flags.nav_cmd_loaded = true;
            }
            // save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
            // and prevent history being re-written until vehicle returns to interrupted position
            if (mission._repeat_dist > 0 && !mission._flags.resuming_mission && mission._nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(mission._nav_cmd.content.location.lat == 0 && mission._nav_cmd.content.location.lng == 0)) {
                // update mission history. last index position is always the most recent wp loaded.
                for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
                    mission._wp_index_history[i] = mission._wp_index_history[i+1];
                }
                mission._wp_index_history[AP_MISSION_MAX_WP_HISTORY-1] = mission._nav_cmd.index;
            }
            // check if the vehicle is resuming and has returned to where it was interrupted
            if (mission._flags.resuming_mission && mission._nav_cmd.index == mission._wp_index_history[AP_MISSION_MAX_WP_HISTORY-1]) {
                // vehicle has resumed previous position
                gcs_send_text(MAV_SEVERITY_INFO, "Mission: Returned to interrupted WP");
                mission._flags.resuming_mission = false;
            }

        } else {
            // set current do command and start it (if not already set)
            if (!mission._flags.do_cmd_loaded) {
                mission._do_cmd = cmd;
                mission._flags.do_cmd_loaded = true;
                mission_start_command(&mission._do_cmd);
            }
        }
        // move onto next command
        cmd_index = cmd.index+1;
    }

    if (max_loops == 0) {
        // infinite loop.  This can happen if there's a loop involving
        // only nav commands (no DO commands) which won't start()
        return false;
    }

    // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
    if (!mission._flags.do_cmd_loaded) {
        mission._flags.do_cmd_all_done = true;
    }

    // if we got this far we must have successfully advanced the nav command
    return true;
}

/// advance_current_do_cmd - moves current do command forward
///     accounts for do-jump commands
static void mission_advance_current_do_cmd()
{
    // exit immediately if we're not running or we've completed all possible "do" commands
    if (mission._flags.state != MISSION_RUNNING || mission._flags.do_cmd_all_done) {
        return;
    }

    // get starting point for search
    uint16_t cmd_index = mission._do_cmd.index;
    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
    } else {
        // start from one position past the current do command
        cmd_index = mission._do_cmd.index + 1;
    }

    // find next do command
    struct Mission_Command cmd;
    if (!mission_get_next_do_cmd(cmd_index, &cmd)) {
        // set flag to stop unnecessarily searching for do commands
        mission._flags.do_cmd_all_done = true;
        return;
    }

    // set current do command and start it
    mission._do_cmd = cmd;
    mission._flags.do_cmd_loaded = true;
    mission_start_command(&mission._do_cmd);
}

/// get_next_cmd - gets next command found at or after start_index
///     returns true if found, false if not found (i.e. mission complete)
///     accounts for do_jump commands
///     increment_jump_num_times_if_found should be set to true if advancing the active navigation command
// bool send_gcs_msg = true
static bool mission_get_next_cmd(uint16_t start_index, mission_command_t cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg)
{
    uint16_t cmd_index = start_index;
    struct Mission_Command temp_cmd;
    uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE;

    // search until the end of the mission command list
    uint8_t max_loops = 64;
    while (cmd_index < (unsigned)mission._cmd_total) {
        // load the next command
        if (!mission_read_cmd_from_storage(cmd_index, &temp_cmd)) {
            // this should never happen because of check above but just in case
            return false;
        }

        // check for do-jump command
        if (temp_cmd.id == MAV_CMD_DO_JUMP) {

            if (max_loops-- == 0) {
                return false;
            }

            // check for invalid target
            if ((temp_cmd.content.jump.target >= (unsigned)mission._cmd_total) || (temp_cmd.content.jump.target == 0)) {
                // To-Do: log an error?
                return false;
            }

            // check for endless loops
            if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
                // we have somehow reached this jump command twice and there is no chance it will complete
                // To-Do: log an error?
                return false;
            }

            // record this command so we can check for endless loops
            if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
                jump_index = cmd_index;
            }

            // check if jump command is 'repeat forever'
            if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
                // continue searching from jump target
                cmd_index = temp_cmd.content.jump.target;
            } else {
                // get number of times jump command has already been run
                int16_t jump_times_run = mission_get_jump_times_run(&temp_cmd);
                if (jump_times_run < temp_cmd.content.jump.num_times) {
                    // update the record of the number of times run
                    if (increment_jump_num_times_if_found && !mission._flags.resuming_mission) {
                        mission_increment_jump_times_run(&temp_cmd, send_gcs_msg);
                    }
                    // continue searching from jump target
                    cmd_index = temp_cmd.content.jump.target;
                } else {
                    // jump has been run specified number of times so move search to next command in mission
                    cmd_index++;
                }
            }
        } else {
            // this is a non-jump command so return it
            *cmd = temp_cmd;
            return true;
        }
    }

    // if we got this far we did not find a navigation command
    return false;
}

/// get_next_do_cmd - gets next "do" or "conditional" command after start_index
///     returns true if found, false if not found
///     stops and returns false if it hits another navigation command before it finds the first do or conditional command
///     accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
static bool mission_get_next_do_cmd(uint16_t start_index, mission_command_t cmd)
{
    struct Mission_Command temp_cmd;

    // check we have not passed the end of the mission list
    if (start_index >= (unsigned)mission._cmd_total) {
        return false;
    }

    // get next command
    if (!mission_get_next_cmd(start_index, &temp_cmd, false, true)) {
        // no more commands so return failure
        return false;
    } else if (mission_is_nav_cmd(&temp_cmd)) {
        // if it's a "navigation" command then return false because we do not progress past nav commands
        return false;
    } else {
        // this must be a "do" or "conditional" and is not a do-jump command so return it
        *cmd = temp_cmd;
        return true;
    }
}

static bool mission_start_command(const mission_command_t cmd)
{
    // check for landing related commands and set in_landing_sequence flag
    if (mission_is_landing_type_cmd(cmd->id) || cmd->id == MAV_CMD_DO_LAND_START) {
        mission._flags.in_landing_sequence = true;
    } else if (mission_is_takeoff_type_cmd(cmd->id)) {
        mission._flags.in_landing_sequence = false;
    }

    gcs_send_text(MAV_SEVERITY_INFO, "Mission: %u %s", cmd->index, mission_command_type(cmd));

    switch (cmd->id) {
    default:
        return mission._cmd_start_fn(cmd);
    }
}

static bool mission_verify_command(const mission_command_t cmd)
{
    switch (cmd->id) {
    // do-commands always return true for verify:
    case MAV_CMD_DO_GRIPPER:
    case MAV_CMD_DO_SET_SERVO:
    case MAV_CMD_DO_SET_RELAY:
    case MAV_CMD_DO_REPEAT_SERVO:
    case MAV_CMD_DO_REPEAT_RELAY:
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
    case MAV_CMD_DO_PARACHUTE:
    case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
    case MAV_CMD_DO_SPRAYER:
    case MAV_CMD_DO_AUX_FUNCTION:
    case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
        return true;
    default:
        return mission._cmd_verify_fn(cmd);
    }
}

// check if command is a landing type command.
static bool mission_is_landing_type_cmd(uint16_t id)
{
    switch (id) {
    case MAV_CMD_NAV_LAND:
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_DO_PARACHUTE:
        return true;
    default:
        return false;
    }
}

// check if command is a takeoff type command.
static bool mission_is_takeoff_type_cmd(uint16_t id)
{
    switch (id) {
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        return true;
    default:
        return false;
    }
}

// calculate the location of the _resume_cmd wp and set as current
static bool mission_calc_rewind_pos(mission_command_t rewind_cmd)
{
    // check for a recent history
    if (mission._wp_index_history[LAST_WP_PASSED] == AP_MISSION_CMD_INDEX_NONE) {
        // no saved history so can't rewind
        return false;
    }

    // check that we have a valid exit position
    if (mission._exit_position.lat == 0 && mission._exit_position.lng == 0) {
        return false;
    }

    struct Mission_Command temp_cmd;
    float rewind_distance = mission._repeat_dist; //(m)
    uint16_t resume_index;
    Location prev_loc = mission._exit_position;

    for (int8_t i = (LAST_WP_PASSED); i>=0; i--) {

        // to get this far there has to be at least one 'passed wp' stored in history.  This is to check incase
        // of history array no being completely filled with valid waypoints upon resume.
        if (mission._wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) {
            // no more stored history
            resume_index = i+1;
            break;
        }

        if (!mission_read_cmd_from_storage(mission._wp_index_history[i], &temp_cmd)) {
            // if read from storage failed then don't use rewind
            return false;
        }

        // calculate distance
        float disttemp = location_get_distance(&prev_loc, &temp_cmd.content.location); //(m)
        rewind_distance -= disttemp;
        resume_index = i;

        if (rewind_distance <= 0.0f) {
            // history rewound enough distance to meet _repeat_dist requirement
            *rewind_cmd = temp_cmd;
            break;
        }

        // store wp location as previous
        prev_loc = temp_cmd.content.location;
    }

    if (rewind_distance > 0.0f) {
        // then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance.
        // the last read temp_cmd will be the furthest cmd back in the history array so resume to that.
        *rewind_cmd = temp_cmd;
        return true;
    }

    // if we have got this far the desired rewind distance lies between two waypoints stored in history array.
    // calculate the location for the mission to resume

    // the last wp read from storage is the wp that is before the resume wp in the mission order
    Location passed_wp_loc = temp_cmd.content.location;

    // fetch next destination wp
    if (!mission_read_cmd_from_storage(mission._wp_index_history[resume_index+1], &temp_cmd)) {
        // if read from storage failed then don't use rewind
        return false;
    }

    // determine the length of the mission leg that the resume wp lies in
    float leg_length = location_get_distance(&passed_wp_loc, &temp_cmd.content.location); //(m)

    // calculate the percentage along the leg that resume wp will be positioned
    float leg_percent = fabsf(rewind_distance)/leg_length;

    // calculate difference vector of mission leg
    Vector3f_t dist_vec = location_get_distance_ned(&passed_wp_loc, &temp_cmd.content.location);

    // calculate the resume wp position
    location_offset(&rewind_cmd->content.location, dist_vec.x * leg_percent, dist_vec.y * leg_percent);
    rewind_cmd->content.location.alt -= dist_vec.z * leg_percent * 100; //(cm)

    // The rewind_cmd.index has the index of the 'last passed wp' from the history array.  This ensures that the mission order
    // continues as planned without further intervention.  The resume wp is not written to memory so will not perminantely change the mission.

    // if we got this far then mission rewind position was successfully calculated
    return true;
}


/// check if the next nav command is a takeoff, skipping delays
static bool mission_is_takeoff_next(uint16_t cmd_index)
{
    struct Mission_Command cmd = {0};
    // check a maximum of 16 items, remembering that missions can have
    // loops in them
    for (uint8_t i=0; i<16; i++, cmd_index++) {
        if (!mission_get_next_nav_cmd(cmd_index, &cmd)) {
            return false;
        }
        switch (cmd.id) {
        // any of these are considered a takeoff command:
        case MAV_CMD_NAV_VTOL_TAKEOFF:
        case MAV_CMD_NAV_TAKEOFF:
        case MAV_CMD_NAV_TAKEOFF_LOCAL:
            return true;
        // any of these are considered "skippable" when determining if
        // we "start with a takeoff command"
        case MAV_CMD_DO_AUX_FUNCTION:
        case MAV_CMD_NAV_DELAY:
            continue;
        default:
            return false;
        }
    }
    return false;
}

// store the latest reported position incase of mission exit and rewind resume
static void mission_update_exit_position(void)
{
    if (!ahrs_get_location(get_ahrs_view(), &mission._exit_position)) {
        mission._exit_position.lat = 0;
        mission._exit_position.lng = 0;
    }
}

// Approximate the distance travelled to get to a landing.  DO_JUMP commands are observed in look forward.
static bool mission_distance_to_landing(uint16_t index, float *tot_distance, Location prev_loc)
{
    struct Mission_Command temp_cmd;
    *tot_distance = 0.0f;
    bool ret;

    // back up jump tracking to reset after distance calculation
    struct jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        _jump_tracking_backup[i] = mission._jump_tracking[i];
    }

    // run through remainder of mission to approximate a distance to landing
    for (uint8_t i=0; i<255; i++) {
        // search until the end of the mission command list
        for (uint16_t cmd_index = index; cmd_index < (unsigned)mission._cmd_total; cmd_index++) {
            // get next command
            if (!mission_get_next_cmd(cmd_index, &temp_cmd, true, false)) {
                // we got to the end of the mission
                ret = false;
                goto reset_do_jump_tracking;
            }
            if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT || mission_is_landing_type_cmd(temp_cmd.id)) {
                break;
            } else if (mission_is_nav_cmd(&temp_cmd) || temp_cmd.id == MAV_CMD_CONDITION_DELAY) {
                // if we receive a nav command that we dont handle then give up as cant measure the distance e.g. MAV_CMD_NAV_LOITER_UNLIM
                ret = false;
                goto reset_do_jump_tracking;
            }
        }
        index = temp_cmd.index+1;

        if (!(temp_cmd.content.location.lat == 0 && temp_cmd.content.location.lng == 0)) {
            // add distance to running total
            float disttemp = location_get_distance(&prev_loc, &temp_cmd.content.location);
            *tot_distance = (*tot_distance) + disttemp;

            // store wp location as previous
            prev_loc = temp_cmd.content.location;
        }

        if (mission_is_landing_type_cmd(temp_cmd.id)) {
            // reached a landing!
            ret = true;
            goto reset_do_jump_tracking;
        }
    }
    // reached end of loop without getting to a landing
    ret = false;

reset_do_jump_tracking:
    for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
        mission._jump_tracking[i] = _jump_tracking_backup[i];
    }

    return ret;
}

static bool mission_calculate_contains_terrain_alt_items(void)
{
    for (int i = 1; i < mission._cmd_total; i++) {
        struct Mission_Command tmp;
        if (!mission_read_cmd_from_storage(i, &tmp)) {
            continue;
        }
        if (mission_stored_in_location(tmp.id) && tmp.content.location.terrain_alt) {
            return true;
        }
    }
    return false;
}

/*------------------------------------test------------------------------------*/


